#!/usr/bin/env python3

from launch import LaunchDescription
from launch_ros.actions import Node
import os

def generate_launch_description():
    # Define the playground path
    playground_path = "/home/yz/Code/EPSILON/simulator/playgrounds/highway_v1.0"
    
    # Define the exact paths to the configuration files
    vehicle_info_path = os.path.join(playground_path, "vehicle_set.json")
    map_path = os.path.join(playground_path, "obstacles_norm.json")
    lane_net_path = os.path.join(playground_path, "lane_net_norm.json")
    
    # Check if files exist
    print(f"Vehicle info path: {vehicle_info_path}, exists: {os.path.exists(vehicle_info_path)}")
    print(f"Map path: {map_path}, exists: {os.path.exists(map_path)}")
    print(f"Lane net path: {lane_net_path}, exists: {os.path.exists(lane_net_path)}")
    
    # Create the phy_simulator node with direct parameter assignment
    phy_simulator_node = Node(
        package='phy_simulator',
        executable='phy_simulator_planning_node',
        name='phy_simulator',
        parameters=[{
            'use_sim_time': True,
            'vehicle_info_path': vehicle_info_path,
            'map_path': map_path,
            'lane_net_path': lane_net_path
        }],
        remappings=[
            ('~/output/arena_info_static', '/map/arena_info_static'),
            ('~/output/arena_info_dynamic', '/perception/arena_info_dynamic'),
            ('~/input/control_signal', '/control/control_signal')
        ],
        output='screen'
    )
    
    return LaunchDescription([
        phy_simulator_node
    ])